#!/usr/bin/env python
"""
1.初始化节点，
2.订阅键盘控制节点
3.打印出键盘输入的消息

4.接受键盘输入的节点后可以判断需要做相应的前进后退的操作。
5.需要导入python文件进行执行相应的代码,需要获取导入python代码的相对路径。
"""
import rospy
from std_msgs.msg import String
import os
import sys
import threading
path = os.path.abspath(".")
# 核心
sys.path.insert(0,path + "/src/car_pkg/scripts")
import USART
import all_socket
import pickle
import threading
import GPS_SEND
import camera

#初始化数组和串口
received_array=[0xFF,1,1,1,1,0xAA]
sender = USART.SerialSender("/dev/ttyS3", 9600)
print("/dev/ttyS3;9600")

#接收键盘节点信息进行相应操作。
def callback(data):
    if data.data == 'w':
        rospy.loginfo("Received key: %s", data.data)
        received_array[0]=0xFF
        received_array[1]=0X02
        received_array[2]=1
        received_array[3]=0
        received_array[4]=0x01
        print(received_array)
        sender.air_distance(received_array)
        print("向前走")
    if data.data == 's':
        rospy.loginfo("Received key: %s", data.data)
        received_array[0]=0xFF
        received_array[1]=0X02
        received_array[2]=1
        received_array[3]=0
        received_array[4]=0x02
        print(received_array)
        sender.air_distance(received_array)
        print("向后走")
    if data.data == 'a':
        rospy.loginfo("Received key: %s", data.data)
        received_array[0]=0xFF
        received_array[1]=0X01
        received_array[2]=0x06
        received_array[3]=0
        received_array[4]=0x03
        print(received_array)
        sender.air_distance(received_array)
        print("左转")
    if data.data == 'd':
        rospy.loginfo("Received key: %s", data.data)
        received_array[0]=0xFF
        received_array[1]=0X01
        received_array[2]=0x07
        received_array[3]=0
        received_array[4]=0x04
        print(received_array)
        sender.air_distance(received_array)
        print("右转")
        
# 打印雷达的数据
def lidar_msg_back(msg):
    if msg == "go":
      print(msg.data)
      received_array[0]= 0xFF
      received_array[0]= 0x01
      received_array[0]= 0x00
      received_array[0]= 0x04
      received_array[0]= 0x01
      received_array[0]= 0xAA
      sender.air_speed(received_array)
    elif msg == "yes":
      print(msg.data)
      received_array[0]= 0xFF
      received_array[0]= 0x01
      received_array[0]= 0x00
      received_array[0]= 0x00
      received_array[0]= 0x05
      received_array[0]= 0xAA
      sender.air_speed(received_array)

#定义一个订阅者，用来接收各个发送节点的数据。
def listener():
    #按键键盘信息订阅节点
    rospy.Subscriber('/key_input', String, callback)
    #雷达信息订阅节点
    rospy.Subscriber('/lidar_avoid', String, lidar_msg_back)
    rospy.spin()



#发送GPS数据
def GPS_datasend():
  GPS_SEND.GPS_datasend()


#发送视频数据
def sendcamera():
  camera.camera_send()


#接收小车的控制数据
def recieve_car_message():
  while True:
    # 接收数据
    received_data = all_socket.message_socket.recv(1024).decode("UTF-8")
    # 将接收到的字节流反序列化为数组
    print(received_data)
    print(type(received_data))
    
    #接收到前端的数据后进行使用“,”进行分隔开处理。
    received_array1=received_data.split(",")
    received_array[0] = int(received_array1[0],16)
    received_array[1] = int(received_array1[1],16)
    received_array[2] = int(received_array1[2],16)
    received_array[3] = int(received_array1[3],16)
    received_array[4] = int(received_array1[4],16)
    received_array[5] = int(received_array1[5],16)
    print(received_array)
    print(type(received_array[0]))
    print(type(received_array))
    
    if received_array[1]==0x01:
      sender.air_speed(received_array)
      print("speed:%d.%d"%(received_array[2],received_array[3]))
    elif received_array[1]==0x02:
      sender.air_distance(received_array)
      print("distance:%d.%d"%(received_array[2],received_array[3]))
    elif received_array[1]==0x04:
      sender.air_armmotion(received_array) 
  # 关闭连接
  all_socket.message_socket.close()
  sender.close_serial()


if __name__ == '__main__': 
    rospy.init_node('subscriber', anonymous=True)
    
    # 建立Socket连接
    all_socket.message_socket1()
    all_socket.camera_socket2()
   
    # 创建线程并传递socket对象
    #1.接收前端数据
    recv_data_thread = threading.Thread(target=recieve_car_message)
    #2.发送GPS数据到前端
    send_gps_thread = threading.Thread(target=GPS_datasend)
    #3.发送视频数据到前端
    send_camera_thread = threading.Thread(target=sendcamera)
    #4.在ros程序里跑程序
    ros_listener_thread = threading.Thread(target=listener)
    
    # 启动线程
    recv_data_thread.start()
    send_gps_thread.start()
    send_camera_thread.start()
    ros_listener_thread.start()
    
    """
    # 等待线程完成
    ros_listener_thread.join()  
    recv_data_thread.join()
    send_gps_thread.join()
    send_camera_thread.join()
    """
    
